package robot;



import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.image.CriteriaCollection;


public class RobotMain extends SimpleRobot 
{
    
    //pwm ids for drive motor control
    private int fL = 1; //pwm id for front left motor
    private int fR = 3; //pwm id for front right motor
    private int rL = 2; //pwm id for rear left motor
    private int rR = 4; //pwm id for rear right motor
    private int shooterPort = 5; //pwm id for the shooter
    private int launcher = 1; //solenoid id for the launcher 
    private int compressorPort = 2; // Digital IO port for the Pressure Switch
    private int spikePort = 1; //Relay port for the Spike
    
    //Control declaration
    private Joystick leftJoystick;
    private Joystick rightJoystick;
    private Joystick gamePad;
    
    //Camera declarations
    private AxisCamera camera; 
    private CriteriaCollection collection;	//criteria for analyzing image
    
    private RobotDrive drive; 
    private Kinect autoDrive;
    
    private Shooter shooter;
    private double shooterSpeed;
    private Compressor airCompressor;
    private boolean shooterOnOff = true;
    
    private double previousButtonPress;
    private double delayForButtonPress;
    
    public void robotInit()
    {
        drive = new RobotDrive(fL, fR, rL, rR);
        leftJoystick = new Joystick(2);
        rightJoystick = new Joystick(1);
        
        shooterSpeed = 0;
        shooter = new Shooter(shooterPort, shooterSpeed, launcher);
        autoDrive = new Kinect(1,2);
        
        airCompressor = new Compressor(compressorPort, spikePort);
      
        delayForButtonPress = 2;
        previousButtonPress = 0;
        
        camera = AxisCamera.getInstance("10.17.45.11");
        camera.writeResolution(AxisCamera.ResolutionT.k160x120);
        camera.writeMaxFPS(15);
        camera.writeCompression(20);
        camera.writeColorLevel(50);
        camera.writeBrightness(40);
        collection = new CriteriaCollection();
    }
    
    public void autonomous() 
    {
        System.err.println("Entering autonomous:");
         while (isAutonomous() && isEnabled()) 
         {
            Camera.imageGrab(camera, collection);
            
         }
    }
    
    public void operatorControl() 
    {
        System.err.println("Entering teleop:");
        airCompressor.start();
        System.err.println("Compressor ON");
        while(isOperatorControl() && isEnabled())
        {
            drive.tankDrive (-rightJoystick.getY(), -leftJoystick.getY());
            
            if(gamePad.getRawButton(1)) //decrease the speed
            {
                if(shooter.getSpeed() < 0 && (Timer.getFPGATimestamp() - previousButtonPress >= delayForButtonPress))
                {
                    previousButtonPress = Timer.getFPGATimestamp();
                    shooterSpeed = shooterSpeed + 0.20;
                    shooter.setTargetSpeed(shooterSpeed);
                    System.err.println("Decreasing Shooter Speed: " + shooterSpeed);
                }
            }
            if(gamePad.getRawButton(4)) //increse the speed
            {
                if(shooterSpeed > -1 && (Timer.getFPGATimestamp() - previousButtonPress >= delayForButtonPress))
                {
                    previousButtonPress = Timer.getFPGATimestamp();
                    shooterSpeed = shooterSpeed - 0.20;
                    shooter.setTargetSpeed(shooterSpeed);
                    System.err.println("Increasing Shooter Speed: " + shooterSpeed);
                }
            }
            
            if(rightJoystick.getRawButton(1)) // if the fire button is pressed shoot
            {
                if(shooter.getSpeed() != 0)
                {
                    System.err.println("Firing Shooter");
                    shooter.fire();
                    shooter.reset();
                }
            }
           
            if(leftJoystick.getRawButton(1)) //turn the shooter off and on
            {
                if(Timer.getFPGATimestamp() - previousButtonPress >= delayForButtonPress)
                {
                    previousButtonPress = Timer.getFPGATimestamp();
                    if(shooterOnOff)
                    {
                        System.err.println("Shooter On");
                        shooter.setTargetSpeed(-1);
                        shooterOnOff = false;
                    }
                    else
                    {
                        System.err.println("Shooter Off");
                        shooter.setTargetSpeed(0);
                        shooterOnOff = true;
                    }
                }
            }
            
        }
        
    }
   
   public void test()
   {
       System.err.println("Entering Test:");
       while(isTest() && isEnabled())
       {   
           //if(/*autoDrive.getInstance().getSkeleton().GetTrackState() == Skeleton.tTrackState.kTracked && */autoDrive.getDeadZone()) 
            //{
               drive.tankDrive(-autoDrive.getLeftY(), -autoDrive.getRightY());
            
            //}
            //else
            //{
            //    drive.tankDrive(0,0);
            //    Timer.delay(.01);
            //}
       }
   }
}